#!/usr/bin/python3

import roslib
import rospy
import math
import tf
import geometry_msgs.msg
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray
from scipy.spatial.transform import Rotation as R
from sensor_msgs.msg import LaserScan
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, DeleteModel, DeleteModelRequest

class p903_gazebo_simulation:
	def __init__(self):
		self.spawn_model_service = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
		self.delete_model_service = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)

	def spawn_initial_model(self):
		rf = DeleteModelRequest()
		rf.model_name = 'zhongduan'
		retval = self.delete_model_service(rf)
		print(retval)
		zhongduan_description = rospy.get_param('zhongduan/sim/robot_description_sim')

		# rf.model_name = 'fine_machining'
		# retval = self.delete_model_service(rf)
		# print(retval)
		# pogo_agv_description = rospy.get_param('fine_machining/robot_description_sim')

		rf = SpawnModelRequest()
		rf.model_xml = zhongduan_description
		rf.model_name = 'zhongduan'
		rf.robot_namespace = 'zhongduan/sim'
		rf.reference_frame = 'world'
		rf.initial_pose.position.x = 15.0
		rf.initial_pose.position.y = 0
		rf.initial_pose.position.z = 1.0 

		rf.initial_pose.orientation.x = 0.0 
		rf.initial_pose.orientation.y = 0.0 
		rf.initial_pose.orientation.z = 0.0 
		rf.initial_pose.orientation.w = 1.0 

		retval = self.spawn_model_service(rf)
		print(retval)

		# rf.model_xml = pogo_agv_description
		# rf.model_name = 'fine_machining'
		# rf.robot_namespace = 'fine_machining/digital_twin'
		# rf.reference_frame = 'world'
		# rf.initial_pose.position.x = 0.0
		# rf.initial_pose.position.y = 0
		# rf.initial_pose.position.z = 2.774 

		# rf.initial_pose.orientation.x = 0.0 
		# rf.initial_pose.orientation.y = 0.0 
		# rf.initial_pose.orientation.z = 0.0 
		# rf.initial_pose.orientation.w = 1.0 

		# retval = self.spawn_model_service(rf)
		# print(retval)

	def start_work_simulation(self):
		# read the tf info form the laserscan to the world
		listener = tf.TransformListener()

		# control the lasersky 2 revolute joints
		laser1 = rospy.Publisher('laser1/command', Float64MultiArray, queue_size=10)
		laser2 = rospy.Publisher('laser2/command', Float64MultiArray, queue_size=10)
		laser3 = rospy.Publisher('laser3/command', Float64MultiArray, queue_size=10)
		pogo1     = rospy.Publisher('pogo_group1/command', Float64MultiArray, queue_size=10)
		pogo2     = rospy.Publisher('pogo_group2/command', Float64MultiArray, queue_size=10)
		pogo3     = rospy.Publisher('pogo_group3/command', Float64MultiArray, queue_size=10)
		pogo4     = rospy.Publisher('pogo_group4/command', Float64MultiArray, queue_size=10)
		pogo5     = rospy.Publisher('pogo_group5/command', Float64MultiArray, queue_size=10)
		agv1	=  rospy.Publisher('agv1_group/command', Float64MultiArray, queue_size=10)
		agv2	=  rospy.Publisher('agv2_group/command', Float64MultiArray, queue_size=10)
		time.sleep(0.5)

		# home point
		laser1.publish(Float64MultiArray(data=[0, 0]))
		laser2.publish(Float64MultiArray(data=[0, 0]))
		laser3.publish(Float64MultiArray(data=[0, 0]))
		pogo1.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
		pogo2.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
		pogo3.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
		pogo4.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
		pogo5.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
		agv1.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
		agv2.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0]))
		time.sleep(1)
		print("home point finish")

		# # # No.1 laser phase
		# input("Input 1 to start laser phase!")
		# laser1_joint=[0.0,0.0]
		# laser2_joint=[0.0,0.0]
		# laser1.publish(Float64MultiArray(data=laser1_joint))
		# laser2.publish(Float64MultiArray(data=laser2_joint))
		# print("laser scan started")

		# # must sleep 1 second
		# time.sleep(1)
		# (trans1,rot1) = listener.lookupTransform('/laser1_link2', '/world', rospy.Time(0))
		# (trans2,rot2) = listener.lookupTransform('/laser2_link2', '/world', rospy.Time(0))

		# # target points' position array in world 
		# target1_p_all_x = [0.7, 0.7, 0.7, 0.0]   #for sky_laser1
		# target1_p_all_y = [0.1, 0.0, 0.2, 0.0]
		# target1_p_all_z = [1.9, 2.1, 2.3, 0.0]
		# target2_p_all_x = [1.0, 1.0, 1.0, 0.0]   #for sky_laser2
		# target2_p_all_y = [0.62,0.61,0.6, 0.0]
		# target2_p_all_z = [1.9, 2.1 ,2.3, 0.0]
		# for i in range(len(target1_p_all_x)):
		# 	target1_p_w = np.array([[target1_p_all_x[i]],[target1_p_all_y[i]],[target1_p_all_z[i]]])
		# 	R1 = R.from_quat(rot1)
		# 	R1 = R1.as_matrix()
		# 	target1_p_h1 = np.matmul(R1, target1_p_w)
		# 	x1 = trans1[0] + target1_p_h1[0][0]
		# 	y1 = trans1[1] + target1_p_h1[1][0]
		# 	z1 = trans1[2] + target1_p_h1[2][0]
		# 	z1=-z1
		# 	dist1 = math.sqrt(x1*x1 + y1*y1 + z1*z1)
		# 	laser1_angle1 = math.atan(y1/x1)
		# 	laser1_angle2 = math.atan(z1/math.sqrt(x1*x1 + y1*y1))
		# 	laser1_joint[0]=laser1_angle1
		# 	laser1_joint[1]=laser1_angle2
		# 	#print(x1, y1, z1, laser1_joint)
		# 	if (abs(laser1_angle1) >= 0.001 or abs(laser1_angle2) >= 0.001) and x1 >= 0:
		# 		laser1.publish(Float64MultiArray(data=laser1_joint))
		# 	elif (abs(laser1_angle1) >= 0.001 or abs(laser1_angle2) >= 0.001) and x1 < 0:
		# 		laser1_joint[0]=laser1_joint[0]+math.pi
		# 		laser1.publish(Float64MultiArray(data=laser1_joint))	
		# 	time.sleep(2)
		# for i in range(len(target2_p_all_x)):
		# 	target2_p_w = np.array([[target2_p_all_x[i]],[target2_p_all_y[i]],[target2_p_all_z[i]]])
		# 	R2 = R.from_quat(rot2)
		# 	R2 = R2.as_matrix()
		# 	target2_p_h2 = np.matmul(R2, target2_p_w)
		# 	x2 = trans2[0] + target2_p_h2[0][0]
		# 	y2 = trans2[1] + target2_p_h2[1][0]
		# 	z2 = trans2[2] + target2_p_h2[2][0]
		# 	z2=-z2
		# 	dist2 = math.sqrt(x2*x2 + y2*y2 + z2*z2)
		# 	laser2_angle1 = math.atan(y2/x2)
		# 	laser2_angle2 = math.atan(z2/math.sqrt(x2*x2 + y2*y2))
		# 	laser2_joint[0]=laser2_angle1
		# 	laser2_joint[1]=laser2_angle2
		# 	#print(x2, y2, z2, laser2_joint)
		# 	if (abs(laser2_angle1) >= 0.001 or abs(laser2_angle2) >= 0.001) and x1 >= 0:
		# 		laser2.publish(Float64MultiArray(data=laser2_joint))
		# 	elif (abs(laser2_angle1) >= 0.001 or abs(laser2_angle2) >= 0.001) and x1 < 0:
		# 		laser1_joint[0]=laser1_joint[0]+math.pi
		# 		laser2.publish(Float64MultiArray(data=[laser2_joint]))	
		# 	time.sleep(2)

		# test agv move
		startx=0.0
		targetx=-1
		step=10000
		for i in range(step):
			xi=startx+(targetx-startx)/step*i
			agv2.publish(Float64MultiArray(data=[xi, 0, 0, 0, 0, 0, 0, 0]))
			time.sleep(0.025)
			print(xi)

		# # # No.3 pogo phase
		# input("Input 3 to start pogoup phase!")
		# # firstly pogo_group1 go up
		# startz=0.0
		# targetz=0.4
		# step=4000
		# for i in range(step):
		# 	zi=startz+(targetz-startz)/step*i
		# 	pogo1.publish(Float64MultiArray(data=[0, 0, zi, 0, 0, zi, 0, 0, zi, 0, 0, zi]))
		# 	time.sleep(0.025)
		# 	print(zi)
		# time.sleep(5)
		# # second phase pogo_group3 go ahead
		# startx=0.0
		# targetx=-0.1
		# step=2000
		# for i in range(step):
		# 	xi=startx+(targetx-startx)/step*i
		# 	pogo1.publish(Float64MultiArray(data=[xi, 0, 0.4, xi, 0, 0.4, xi, 0, 0.4, xi, 0, 0.4]))
		# 	time.sleep(0.025)  
		# 	print(xi)

if __name__ == '__main__':
	# init the node
	rospy.init_node('simulation_demo', anonymous=True)
	simu = p903_gazebo_simulation()
	simu.spawn_initial_model()
	rospy.spin()
